#include "my_math.h"
#include "main.h"

float dis = 0.f;
float l1,l2;
float angle, angle_offset;
uint8_t math_step = 0;

/**
 * @brief  初始化
 * @param  
 * @retval 无
 */
void my_math_init(void)
{
	
}

/**
 * @brief  余弦定理
 * @param  
 * @retval 无
 */
void CosineTheorem(void)
{
	dis = sqrt(l1*l1 + l2*l2 - 2*l1*l2*cosf(angle));
}

/**
 * @brief  
 * @param  
 * @retval 无
 */
void my_math_update(void)
{
	if (getkey_led(0)) {

		if ( ++math_step > 2) {
			buzz_didi(1);
			
			math_step = 0;
			dis = 0;
			OLED_Clear();
		};
	}
	
	/* 选中第一个步骤 */
	if (math_step == 0) {
		l1 = laser_dis;
		l2 = 0.f;
		
		angle = adc_driver.angle;
		angle_offset = angle;
	}
	
	/* 第二步 */
	if (math_step == 1) {
		l2 = laser_dis;
		
		angle = fabs(adc_driver.angle - angle_offset);
		
//		if (angle < 0) {
//			angle += _2PI;
//		}
//		
//		if (angle > _2PI) {
//			angle -= _2PI;
//		}
		
		CosineTheorem();
	}
}

/******************* (C) COPYRIGHT 2025 FUYEBING*****END OF FILE************/
